#include "EMatrix4x4.h"
#include <assert.h>

/**
 * Returns data at specified row and column. Note that this is a column-major implementation.
 * The first 4 members in memory describe side vector and further ones describe up, forward and translation.
 * @ returns Reference (!) to required matrix element.
 */
template <typename TYPE>
inline TYPE& Matrix4x4<TYPE>::at(uint row, uint column)
{
	assert(row < 4 && column < 4);
	return data2D[column][row];
}

/**
 * Returns data at specified row and column. Note that this is a column-major implementation.
 * The first 4 members in memory describe side vector and further ones describe up, forward and translation.
 * @ returns Value of required matrix element.
 */
template <typename TYPE>
inline TYPE Matrix4x4<TYPE>::get(uint row, uint column) const
{
	assert(row < 4 && column < 4);
	return data2D[column][row];
}

/**
 * Returns data at specified row and column. Note that this is a column-major implementation.
 * The first 4 members in memory describe side vector and further ones describe up, forward and translation.
 * @ returns Value of required matrix element.
 */
template <typename TYPE>
inline void Matrix4x4<TYPE>::set(uint row, uint column, TYPE value)
{
	assert(row < 4 && column < 4);
	data2D[column][row] = value;
}

template <typename TYPE>
Matrix4x4<TYPE>::Matrix4x4()
{
	this->setIdentity();
}

template <typename TYPE>
Matrix4x4<TYPE>::Matrix4x4(const Matrix4x4& other)
{
	for (uint r = 0; r < 4; ++r)
	{
		for (uint c = 0; c < 4; ++c)
			this->at(r, c) = other.get(r, c);
	}
}

template <typename TYPE>
Matrix4x4<TYPE>::Matrix4x4(const Vector3<TYPE>& translation) ///< create matrix from translation vector
{
	this->at(0, 0) 	= 1;
	this->at(1, 0) 	= 0;
	this->at(2, 0) 	= 0;
	this->at(3, 0) 	= 0;

	this->at(0, 1) 	= 0;
	this->at(1, 1) 	= 1;
	this->at(2, 1) 	= 0;
	this->at(3, 1) 	= 0;

	this->at(0, 2) 	= 0;
	this->at(1, 2) 	= 0;
	this->at(2, 2) 	= 1;
	this->at(3, 2) 	= 0;

	this->at(0, 3) 	= translation.x;
	this->at(1, 3) 	= translation.y;
	this->at(2, 3) 	= translation.z;
	this->at(3, 3) 	= 1;
}

template <typename TYPE>
Matrix4x4<TYPE> Matrix4x4<TYPE>::getScalingMatrix(Vector3<TYPE> scale)
{
	Matrix4x4<TYPE> matrix;
	matrix.at(0, 0) = scale.x;
	matrix.at(1, 1) = scale.y;
	matrix.at(2, 2) = scale.z;
	return matrix;
}

template <typename TYPE>
Matrix4x4<TYPE> Matrix4x4<TYPE>::getRotationMatrix(Quaternion<TYPE> q)
{
	// TODO: method should be const.
	q.normalize();

	// http://content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation
	// http://gamedev.stackexchange.com/questions/34519/rotation-matrix-derived-from-quaternion-is-opposite-of-expected-direction
	TYPE x2 = q.x * q.x;
	TYPE y2 = q.y * q.y;
	TYPE z2 = q.z * q.z;
	TYPE xy = q.x * q.y;
	TYPE xz = q.x * q.z;
	TYPE yz = q.y * q.z;
	TYPE wx = q.w * q.x;
	TYPE wy = q.w * q.y;
	TYPE wz = q.w * q.z;

	Matrix4x4<TYPE> matrix;
	// This calculation would be a lot more complicated for non-unit length quaternions.
	matrix.at(0, 0) = 1.0 - 2.0 * (y2 + z2),
	matrix.at(1, 0) = 2.0 * (xy + wz),
	matrix.at(2, 0) = 2.0 * (xz - wy),
	matrix.at(3, 0) = 0.0,

	matrix.at(0, 1) = 2.0 * (xy - wz),
	matrix.at(1, 1) = 1.0 - 2.0 * (x2 + z2),
	matrix.at(2, 1) = 2.0 * (yz + wx),
	matrix.at(3, 1) = 0.0,

	matrix.at(0, 2) = 2.0 * (xz + wy),
	matrix.at(1, 2) = 2.0 * (yz - wx),
	matrix.at(2, 2) = 1.0 - 2.0 * (x2 + y2),
	matrix.at(3, 2) = 0.0,

	matrix.at(0, 3) = 0.0,
	matrix.at(1, 3) = 0.0,
	matrix.at(2, 3) = 0.0,
	matrix.at(3, 3) = 1.0;

	//matrix.transpose();

	return matrix;
}

template <typename TYPE>
Matrix4x4<TYPE> Matrix4x4<TYPE>::getTranslationMatrix(Vector3<TYPE> translation)
{
	Matrix4x4<TYPE> matrix;
	matrix.at(0, 3) = translation.x;
	matrix.at(1, 3) = translation.y;
	matrix.at(2, 3) = translation.z;
	return matrix;
}

template <typename TYPE>
Matrix3x3<TYPE> Matrix4x4<TYPE>::get3x3() const
{
	Matrix3x3<TYPE> ret;
	for (uint r = 0; r < 3; ++r)
	{
		for (uint c = 0; c < 3; ++c)
			ret.at(r, c) = this->get(r, c);
	}
	return ret;
}

template <typename TYPE>
Matrix4x4<TYPE>& Matrix4x4<TYPE>::setIdentity()
{
	this->at(0, 0) 	= 1;
	this->at(1, 0) 	= 0;
	this->at(2, 0) 	= 0;
	this->at(3, 0) 	= 0;

	this->at(0, 1) 	= 0;
	this->at(1, 1) 	= 1;
	this->at(2, 1) 	= 0;
	this->at(3, 1) 	= 0;

	this->at(0, 2) 	= 0;
	this->at(1, 2) 	= 0;
	this->at(2, 2) 	= 1;
	this->at(3, 2) 	= 0;

	this->at(0, 3) 	= 0;
	this->at(1, 3) 	= 0;
	this->at(2, 3) 	= 0;
	this->at(3, 3) 	= 1;

	return *this;
}

template <typename TYPE>
Matrix4x4<TYPE> Matrix4x4<TYPE>::getIdentity()
{
	Matrix4x4<TYPE> ret;
	return ret.setIdentity();
}

template <typename TYPE>
Matrix4x4<TYPE>& Matrix4x4<TYPE>::transpose()
{
	// TODO: Optimize
	Matrix4x4<TYPE> copy = *this;
	for (int r = 0; r < 4; ++r)
	{
		for (int c = 0; c < 4; ++c)
			this->at(r, c) = copy.at(c, r);
	}
	return *this;
}

template <typename TYPE>
Matrix4x4<TYPE> Matrix4x4<TYPE>::getTransposed() const
{
	Matrix4x4<TYPE> ret;

	for (int r = 0; r < 4; ++r)
	{
		for (int c = 0; c < 4; ++c)
			ret.at(r, c) = this->get(c, r);
	}

	return ret;
}

template <typename TYPE>
Matrix4x4<TYPE> Matrix4x4<TYPE>::invert()
{
	// http://stackoverflow.com/questions/1148309/inverting-a-4x4-matrix
	Matrix4x4<TYPE> copy = *this;

	// HACK: I don't have time for this...
	#ifdef m__
		#error "wat"
	#else
		#define m__(x)	(copy.at(x%4, x/4))
		#define i__(x)	(this->at(x%4, x/4))
	#endif

	i__(0) =
		m__(5)  * m__(10) * m__(15) -
		m__(5)  * m__(11) * m__(14) -
		m__(9)  * m__(6)  * m__(15) +
		m__(9)  * m__(7)  * m__(14) +
		m__(13) * m__(6)  * m__(11) -
		m__(13) * m__(7)  * m__(10);

	i__(4) =
		-m__(4)  * m__(10) * m__(15) +
		m__(4)  * m__(11) * m__(14) +
		m__(8)  * m__(6)  * m__(15) -
		m__(8)  * m__(7)  * m__(14) -
		m__(12) * m__(6)  * m__(11) +
		m__(12) * m__(7)  * m__(10);

	i__(8) =
		m__(4)  * m__(9) * m__(15) -
		m__(4)  * m__(11) * m__(13) -
		m__(8)  * m__(5) * m__(15) +
		m__(8)  * m__(7) * m__(13) +
		m__(12) * m__(5) * m__(11) -
		m__(12) * m__(7) * m__(9);

	i__(12) =
		-m__(4)  * m__(9) * m__(14) +
		m__(4)  * m__(10) * m__(13) +
		m__(8)  * m__(5) * m__(14) -
		m__(8)  * m__(6) * m__(13) -
		m__(12) * m__(5) * m__(10) +
		m__(12) * m__(6) * m__(9);

	i__(1) =
		-m__(1)  * m__(10) * m__(15) +
		m__(1)  * m__(11) * m__(14) +
		m__(9)  * m__(2) * m__(15) -
		m__(9)  * m__(3) * m__(14) -
		m__(13) * m__(2) * m__(11) +
		m__(13) * m__(3) * m__(10);

	i__(5) =
		m__(0)  * m__(10) * m__(15) -
		m__(0)  * m__(11) * m__(14) -
		m__(8)  * m__(2) * m__(15) +
		m__(8)  * m__(3) * m__(14) +
		m__(12) * m__(2) * m__(11) -
		m__(12) * m__(3) * m__(10);

	i__(9) =
		-m__(0)  * m__(9) * m__(15) +
		m__(0)  * m__(11) * m__(13) +
		m__(8)  * m__(1) * m__(15) -
		m__(8)  * m__(3) * m__(13) -
		m__(12) * m__(1) * m__(11) +
		m__(12) * m__(3) * m__(9);

	i__(13) =
		m__(0)  * m__(9) * m__(14) -
		m__(0)  * m__(10) * m__(13) -
		m__(8)  * m__(1) * m__(14) +
		m__(8)  * m__(2) * m__(13) +
		m__(12) * m__(1) * m__(10) -
		m__(12) * m__(2) * m__(9);

	i__(2) =
		m__(1)  * m__(6) * m__(15) -
		m__(1)  * m__(7) * m__(14) -
		m__(5)  * m__(2) * m__(15) +
		m__(5)  * m__(3) * m__(14) +
		m__(13) * m__(2) * m__(7) -
		m__(13) * m__(3) * m__(6);

	i__(6) =
		-m__(0)  * m__(6) * m__(15) +
		m__(0)  * m__(7) * m__(14) +
		m__(4)  * m__(2) * m__(15) -
		m__(4)  * m__(3) * m__(14) -
		m__(12) * m__(2) * m__(7) +
		m__(12) * m__(3) * m__(6);

	i__(10) =
		m__(0)  * m__(5) * m__(15) -
		m__(0)  * m__(7) * m__(13) -
		m__(4)  * m__(1) * m__(15) +
		m__(4)  * m__(3) * m__(13) +
		m__(12) * m__(1) * m__(7) -
		m__(12) * m__(3) * m__(5);

	i__(14) =
		-m__(0)  * m__(5) * m__(14) +
		m__(0)  * m__(6) * m__(13) +
		m__(4)  * m__(1) * m__(14) -
		m__(4)  * m__(2) * m__(13) -
		m__(12) * m__(1) * m__(6) +
		m__(12) * m__(2) * m__(5);

	i__(3) =
		-m__(1) * m__(6) * m__(11) +
		m__(1) * m__(7) * m__(10) +
		m__(5) * m__(2) * m__(11) -
		m__(5) * m__(3) * m__(10) -
		m__(9) * m__(2) * m__(7) +
		m__(9) * m__(3) * m__(6);

	i__(7) =
		m__(0) * m__(6) * m__(11) -
		m__(0) * m__(7) * m__(10) -
		m__(4) * m__(2) * m__(11) +
		m__(4) * m__(3) * m__(10) +
		m__(8) * m__(2) * m__(7) -
		m__(8) * m__(3) * m__(6);

	i__(11) =
		-m__(0) * m__(5) * m__(11) +
		m__(0) * m__(7) * m__(9) +
		m__(4) * m__(1) * m__(11) -
		m__(4) * m__(3) * m__(9) -
		m__(8) * m__(1) * m__(7) +
		m__(8) * m__(3) * m__(5);

	i__(15) =
		m__(0) * m__(5) * m__(10) -
		m__(0) * m__(6) * m__(9) -
		m__(4) * m__(1) * m__(10) +
		m__(4) * m__(2) * m__(9) +
		m__(8) * m__(1) * m__(6) -
		m__(8) * m__(2) * m__(5);

	TYPE determinant = m__(0) * i__(0) + m__(1) * i__(4) + m__(2) * i__(8)  + m__(3) * i__(12);
	assert(determinant != 0);
	determinant = 1.0 / determinant;

	for (uint i = 0; i < 16; ++i)
		i__(i) = i__(i) * determinant;

	#undef m__
	#undef i__

	return *this;
}

template <typename TYPE>
Matrix4x4<TYPE> Matrix4x4<TYPE>::getInverted() const
{
	Matrix4x4<TYPE> copy = *this;
	return copy.invert();
}

template <typename TYPE>
Vector3<TYPE> Matrix4x4<TYPE>::side() const
{
	return Vector3<TYPE>(this->get(0, 0), this->get(1, 0), this->get(2, 0));
}

template <typename TYPE>
Vector3<TYPE> Matrix4x4<TYPE>::up() const
{
	return Vector3<TYPE>(this->get(0, 1), this->get(1, 1), this->get(2, 1));
}

template <typename TYPE>
Vector3<TYPE> Matrix4x4<TYPE>::forward() const
{
	return Vector3<TYPE>(this->get(0, 2), this->get(1, 2), this->get(2, 2));
}

template <typename TYPE>
Vector3<TYPE> Matrix4x4<TYPE>::translation() const
{
	return Vector3<TYPE>(this->get(0, 3), this->get(1, 3), this->get(2, 3));
}

template <typename TYPE>
Matrix4x4<TYPE>& Matrix4x4<TYPE>::translate(Vector3<TYPE> translation)
{
	for (int i = 0; i < 4; i ++)
		this->at(i, 3) += this->at(i, 0) * translation.x + this->at(i, 1) * translation.y + this->at(i, 2) * translation.z;
	return *this;
}

template <typename TYPE>
void Matrix4x4<TYPE>::operator*=(const Matrix4x4<TYPE>& other)
{
	Matrix4x4<TYPE> copy(*this);
	TYPE sum;
	for (uint r = 0; r < 4; ++r)
	{
		for (uint c = 0; c < 4; ++c)
		{
			sum = 0;
			for (uint i = 0; i < 4; ++i)
				sum += copy.get(r, i) * other.get(i, c);
			this->at(r, c) = sum;
		}
	}
}

template <typename TYPE>
Matrix4x4<TYPE> operator*(const Matrix4x4<TYPE>& a, const Matrix4x4<TYPE>& b)
{
    Matrix4x4<TYPE> ret;
    TYPE sum;
    for (uint r = 0; r < 4; ++r)
    {
        for (uint c = 0; c < 4; ++c)
        {
            sum = 0;
            for (uint i = 0; i < 4; ++i)
            	sum += a.get(r, i) * b.get(i, c);
            ret.at(r, c) = sum;
        }
    }
    return ret;
}

INSTANTIATE_MATRIX_TEMPLATE(Matrix4x4);

template Matrix4x4<float> 	operator*(const Matrix4x4<float>&  a, const Matrix4x4<float>&  b);
template Matrix4x4<double> 	operator*(const Matrix4x4<double>& a, const Matrix4x4<double>& b);


